package moro;

import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.Polygon;
import java.awt.geom.Point2D;

/**
 * Title:        The MObile RObot Simulation Environment
 * Description:
 * Copyright:    Copyright (c) 2001
 * Company:      Universit� di Bergamo
 * @author Davide Brugali
 * @version 1.0
 */
public class Platform extends Device {

    int orientation         = 1;      // 1: clockwise  -1: otherwise
    double rotStep          = 1.0;     // one degree
    double moveStep         = 1.0;
    double numRotSteps      = 0;
    double numMoveSteps     = 0;
    private int navLightRef = 26;      // afstand navigatielichten tov referentiepunt 
    private int frontPosRef = 30;      // afstand zoeklicht tov referentiepunt
    private int searchlightDimension = 120;
    private int searchlightAngle = 90;
    
    public Platform(String name, Robot robot, Environment environment) {
        super(name, robot, new Position(0.0, 0.0, 0.0), environment);
        // onderstaande coordinaten leveren de volgende vorm op
        //  _______
        // |       \ 
        // |      + |   sensorpunt-voorzijde   = 10 cm
        // |_______/    sensorpunt-zijkant     = 20 cm
        //              sensorpunt-achterzijde = 40 cm 
        //  lengte 40 + 10 = 50
        //  breedte 40
        this.addPoint(20, 20);
        this.addPoint(30, 10);
        this.addPoint(30, -10);
        this.addPoint(20, -20);
        this.addPoint(-20, -20);
        this.addPoint(-20, 20);

    }
 
    /**
     * Voer van Controller ontvangen opdrachten uit voor het mobiele platform
     * Hier worden dus de bewegingen van P1 uitgevoerd
     * @param command
     */
    @Override
    public void executeCommand(String command) {
        if (command.indexOf("ROTATERIGHT") > -1) {
            double angle = Math.abs(Double.parseDouble(command.trim().substring(12).trim()));
            numRotSteps = angle / rotStep;
            orientation = 1;
            running = true;
        } else if (command.indexOf("ROTATELEFT") > -1) {
            double angle = Math.abs(Double.parseDouble(command.trim().substring(11).trim()));
            numRotSteps = angle / rotStep;
            orientation = -1;
            running = true;
        } else if (command.indexOf("MOVEFW") > -1) {
            double distance = Math.abs(Double.parseDouble(command.trim().substring(7).trim()));
            numMoveSteps = distance / moveStep;
            orientation = 1;
            running = true;
        } else if (command.indexOf("MOVEBW") > -1) {
            double distance = Math.abs(Double.parseDouble(command.trim().substring(7).trim()));
            numMoveSteps = distance / moveStep;
            orientation = -1;
            running = true;
        } else {
            writeOut("DECLINED");
        }
    }
   
    /**
     *  Voer één stap per keer uit
     */
    @Override
    public void nextStep() {
        if (!running) {
            return;
        }

        if (numRotSteps > 0.0) {
            if (numRotSteps < 1.0) {
                robotPos.rototras(0.0, 0.0, numRotSteps * orientation * rotStep);
            } else {
                robotPos.rototras(0.0, 0.0, orientation * rotStep);
            }
            // updates the robot's position
            robot.writePosition(robotPos);


            // redraws the environment
            environment.repaint();
            //------------------------

            numRotSteps -= 1.0;
        } else if (numMoveSteps > 0.0) {
            if (numMoveSteps < 1.0) {
                robotPos.rototras(numMoveSteps * moveStep * orientation, 0.0, 0.0);
            } else {
                robotPos.rototras(moveStep * orientation, 0.0, 0.0);
            }
            // updates the robot's position
            robot.writePosition(robotPos);

            // redraws the environment
            environment.repaint();
            //------------------------

            numMoveSteps -= 1.0;
        } else {
            running = false;
            writeOut("PLATFORM ARRIVED");
        }
    }

    @Override
    public void paint(Graphics g) {
        robot.readPosition(robotPos);

        // draws the shape
        Polygon globalShape = new Polygon();
        Point2D point = new Point2D.Double();

        for (int i = 0; i < shape.npoints; i++) {
            point.setLocation(shape.xpoints[i], shape.ypoints[i]);
            // calculates the coordinates of the point according to the local position
            localPos.rototras(point);
            // calculates the coordinates of the point according to the robot position
            robotPos.rototras(point);
            // adds the point to the global shape
            globalShape.addPoint((int) Math.round(point.getX()), (int) Math.round(point.getY()));
        }

        ((Graphics2D) g).setColor(bgColor);
        ((Graphics2D) g).fillPolygon(globalShape);
        ((Graphics2D) g).setColor(fgColor);
        ((Graphics2D) g).drawPolygon(globalShape);
        ((Graphics2D) g).setColor(txtColor);

        ((Graphics2D) g).setColor(bgColor);
        ((Graphics2D) g).fillPolygon(globalShape);
        ((Graphics2D) g).setColor(fgColor);
        ((Graphics2D) g).drawPolygon(globalShape);
        ((Graphics2D) g).setColor(txtColor);
        // Teken indentificatie op de Robot
        // orientatie is onmogelijk met Drawstring, past toch in alle richtingen precies
        ((Graphics2D) g).drawString("C-3PO", (int) robotPos.getX() - 13, (int) robotPos.getY() + 15);
        
        
        // correctie om Zoeklicht op de juiste plaats op de robot te plaatsen
        double Xpos = (robot.position.getX() + (frontPosRef * Math.cos(robot.position.getT())));
        double Ypos = (robot.position.getY() + (frontPosRef * Math.sin(robot.position.getT())));
        int Tpos = (int) Math.toDegrees(robot.position.getT()) ;
        if(Tpos > 0 && Tpos < 360){
            Tpos = (360-Tpos);
        }
        
        // draw zoeklicht
        if (running) {
            ((Graphics2D) g).setColor(Color.yellow);
            ((Graphics2D) g).fillArc((int) Xpos - (searchlightDimension /2), (int) Ypos - (searchlightDimension / 2),
                    searchlightDimension, searchlightDimension, Tpos - (searchlightAngle / 2), searchlightAngle);
        }
        
        // correctie om Navigatielichten Bakboord en Stuurboord op juiste plaats te houden
        // de referentiepunten liggen +45 en -45 graden tov midden van de robot
        // bij rotatie robot moet dit punt steeds opnieuw worden bepaald. Volgens Brugali vergelijking 10.2
        // toggleswitch, intermitterend, 1 seconde
        if ((seconds + 1000) < System.currentTimeMillis()) {
            seconds = System.currentTimeMillis();
            toggleSwitch = !toggleSwitch;
        }
        
        if (!toggleSwitch) {
            double XposL = (robot.position.getX() + (navLightRef * (Math.cos(robot.position.getT() - ((2 * Math.PI) / 8))))) - robot.position.getX() - 2;
            double YposL = (robot.position.getY() + (navLightRef * (Math.sin(robot.position.getT() - ((2 * Math.PI) / 8))))) - robot.position.getY() - 2;
            ((Graphics2D) g).setColor(Color.RED);
            ((Graphics2D) g).fillOval((int) (Math.round(robot.position.getX() + XposL)), (int) (Math.round(robot.position.getY() + YposL)), 4, 4);

            double XposR = (robot.position.getX() + (navLightRef * (Math.cos(robot.position.getT() + ((2 * Math.PI) / 8))))) - robot.position.getX() - 2 ;
            double YposR = (robot.position.getY() + (navLightRef * (Math.sin(robot.position.getT() + ((2 * Math.PI) / 8))))) - robot.position.getY() - 2;
            ((Graphics2D) g).setColor(Color.GREEN);
            ((Graphics2D) g).fillOval((int) (Math.round(robot.position.getX() + XposR)), (int) (Math.round(robot.position.getY() + YposR)), 4, 4);
        }

        // zwaailicht, knippert tegengesteld aan navigatielichten
        if (toggleSwitch) {
            // x coordinate upper left, y coordinate upper left, width oval, height oval
            ((Graphics2D) g).setColor(Color.ORANGE);
            ((Graphics2D) g).fillOval((int) robot.position.getX() -2, (int) robot.position.getY() -2, 4, 4);

        }
    }
    
    /**
     * not applicable for device of type: platform
     * for sensors types only
     * @return detection Range of device
     */
    @Override
    public int getRange() {
        return 0;
    } 

}
